#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    // tf广播器
    static tf::TransformBroadcaster br;



    // 根据乌龟当前的位姿，设置相对于世界坐标系的坐标变换
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);

    // 发布坐标变换
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

    // 发布固定点坐标
    tf::Transform transform_a;
    transform_a.setOrigin(tf::Vector3(1.0, 6.0, 0.0));
    tf::Quaternion q_a;
    q_a.setRPY(0.0, 0.0, 0.0);
    transform_a.setRotation(q_a);
    br.sendTransform(tf::StampedTransform(transform_a, ros::Time::now(), "world", "point_a"));
    
    tf::Transform transform_b;
    transform_b.setOrigin(tf::Vector3(10.0, 6.0, 0.0));
    tf::Quaternion q_b;
    q_b.setRPY(0.0, 0.0, 0.0);
    transform_b.setRotation(q_b);
    br.sendTransform(tf::StampedTransform(transform_b, ros::Time::now(), "world", "point_b"));
}

int main(int argc, char** argv)
{
    // 初始化节点
    ros::init(argc, argv, "my_tf_broadcaster");
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    };
    turtle_name = argv[1];

    // 订阅乌龟的pose信息
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
    
    // 通过服务调用，产生第二只乌龟turtle2
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);
    
    ros::spin();

    return 0;
};
